#include "user_task.h"
#include "cmd.h"

#ifdef DEBUG

CMD_t cmd;
CMD_RC_t rc_ctrl;
CMD_ACTION_t act;
CMD_NUC_t Nuc;

#endif 

static CMD_t cmd;
static CMD_RC_t rc_ctrl;
static CMD_ACTION_t act;
static CMD_NUC_t Nuc;



void Task_cmd(void *argument){
   (void)argument; /*未使用传递参数 消除警告*/

 /* 计算到下一次调度任务所需要的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CMD;

    /**/
  CMD_Init(&cmd);
	
  uint32_t tick = osKernelGetTickCount(); /*控制任务运行频率的计时 */

   while(1){
#ifdef DEBUG
    /*记录任务所需要的栈空间*/
    task_runtime.stack_water_mark.cmd =
        osThreadGetStackSpace(osThreadGetId());
#endif

    osKernelLock(); /*锁住RTOS内核调度*/

    /*将各任务接收到的原始数据解析为通用的控制命令*/
		if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)
      CMD_ParseRc(&cmd, &rc_ctrl);
		  R12ds_ErrorDect(&cmd,&rc_ctrl);
		//如果下段代码注释掉 会出现遥控器接收数据卡死问题（原因是导航任务中陀螺仪会修正角度会发送控制向量）
//    if(osMessageQueueGet(task_runtime.msgq.cmd.raw.ops_9, &act, 0, 0) == osOK)
//      CMD_ParseAct(&cmd,&act);
    if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)
      CMD_ParseNuc(&cmd,&Nuc);

    osKernelUnlock(); /* 同上 解锁RTOS内核 */
     /*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
     osMessageQueueReset(task_runtime.msgq.cmd.chassis);
     osMessageQueuePut(task_runtime.msgq.cmd.chassis,&cmd,0,0);

//     osMessageQueueReset(task_runtime.msgq.cmd.status);
//     osMessageQueuePut(task_runtime.msgq.cmd.status,&cmd.Status,0,0);
     

    tick += delay_tick; /*计算下一个唤醒时刻*/
    osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */

}
		
}



